// SPDX-FileCopyrightText: The openTCS Authors
// SPDX-License-Identifier: MIT
package org.opentcs.kcvehicle;

import static java.util.Objects.requireNonNull;

import com.google.common.util.concurrent.Uninterruptibles;
import com.google.inject.assistedinject.Assisted;
import jakarta.inject.Inject;
import java.beans.PropertyChangeEvent;
import java.util.Arrays;
import java.util.HashMap;
import java.util.Iterator;
import java.util.List;
import java.util.Objects;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;
import java.util.stream.Collectors;
import org.opentcs.common.LoopbackAdapterConstants;
import org.opentcs.customizations.kernel.KernelExecutor;
import org.opentcs.data.model.Vehicle;
import org.opentcs.data.order.Route.Step;
import org.opentcs.data.order.TransportOrder;
import org.opentcs.drivers.vehicle.BasicVehicleCommAdapter;
import org.opentcs.drivers.vehicle.LoadHandlingDevice;
import org.opentcs.drivers.vehicle.MovementCommand;
import org.opentcs.drivers.vehicle.SimVehicleCommAdapter;
import org.opentcs.drivers.vehicle.VehicleCommAdapter;
import org.opentcs.drivers.vehicle.VehicleProcessModel;
import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
import org.opentcs.kcvehicle.communication.kc.service.Communication;
import org.opentcs.kcvehicle.communication.kc.service.Communications;
import org.opentcs.kcvehicle.communication.kc.service.CommunicationFactory;
import org.opentcs.kcvehicle.communication.kc.udp.Service.ActImmediately;
import org.opentcs.kcvehicle.communication.kc.udp.Service.HybridNavigation;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.af.QueryRobotStatusRsp;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;
import org.opentcs.util.CyclicTask;
import org.opentcs.util.ExplainedBoolean;
import org.opentcs.kcvehicle.VelocityController.WayEntry;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

/**
 * A {@link VehicleCommAdapter} that does not really communicate with a physical vehicle but roughly
 * simulates one.
 */
public class KCLoopbackCommunicationAdapter
    extends
      BasicVehicleCommAdapter
    implements
      SimVehicleCommAdapter {



  /**
   * The time by which to advance the velocity controller per step (in ms).
   */
  private static final int ADVANCE_TIME = 100;

  /**
   * kc通讯适配器.
   */
  private final CommunicationFactory kccommunicationFactory;
  private Communication comm;

  /**
   * af监听器 ，内部类
   */
  private KCVehicleTask kcafVehicleTask;
  private ExecutorService afListenerExecutor = Executors.newSingleThreadExecutor();

  /**
   * The name of the load handling device set by this adapter.
   */
  public static final String LHD_NAME = "default";
  /**
   * This class's Logger.
   */
  private static final Logger LOG = LoggerFactory.getLogger(KCLoopbackCommunicationAdapter.class);
  /**
   * An error code indicating that there's a conflict between a load operation and the vehicle's
   * current load state.
   */
  private static final String LOAD_OPERATION_CONFLICT = "cannotLoadWhenLoaded";
  /**
   * An error code indicating that there's a conflict between an unload operation and the vehicle's
   * current load state.
   */
  private static final String UNLOAD_OPERATION_CONFLICT = "cannotUnloadWhenNotLoaded";
  /**
   * The time (in ms) of a single simulation step.
   */
  private static final int SIMULATION_PERIOD = 100;
  /**
   * This instance's configuration.
   */
  private final KCVirtualVehicleConfiguration configuration;
  /**
   * Indicates whether the vehicle simulation is running or not.
   */
  private volatile boolean isSimulationRunning;
  /**
   * The vehicle to this comm adapter instance.
   */
  private final Vehicle vehicle;
  /**
   * The vehicle's load state.
   */
  private LoadState loadState = LoadState.EMPTY;
  /**
   * Whether the loopback adapter is initialized or not.
   */
  private boolean initialized;

  /**
   * Creates a new instance.
   *
   * @param configuration This class's configuration.
   * @param vehicle The vehicle this adapter is associated with.
   * @param kernelExecutor The kernel's executor.
   */
  @Inject
  public KCLoopbackCommunicationAdapter(
      KCVirtualVehicleConfiguration configuration,
      @Assisted
      Vehicle vehicle,
      @KernelExecutor
      ScheduledExecutorService kernelExecutor,
      CommunicationFactory kccommunicationFactory
  ) {
    super(
        new KCLoopbackVehicleModel(vehicle),
        configuration.commandQueueCapacity(),
        configuration.rechargeOperation(),
        kernelExecutor
    );
    this.vehicle = requireNonNull(vehicle, "vehicle");
    this.configuration = requireNonNull(configuration, "configuration");
    this.kccommunicationFactory = requireNonNull(kccommunicationFactory, "kccommunicationFactory");
  }

  @Override
  public void initialize() {
    if (isInitialized()) {
      return;
    }
    super.initialize();

    String initialPos
        = vehicle.getProperties().get(LoopbackAdapterConstants.PROPKEY_INITIAL_POSITION);
    if (initialPos != null) {
      initVehiclePosition(initialPos);
    }
    getProcessModel().setState(Vehicle.State.IDLE);
    getProcessModel().setLoadHandlingDevices(
        Arrays.asList(new LoadHandlingDevice(LHD_NAME, false))
    );

    //初始化 kcCommunication 通讯
    initialKCCommunication();
    initialized = true;
  }

  private void initialKCCommunication() {
    //todo 初始化 kcCommunication
    //实例化 kc 通讯链接
    //开始
    //vehicle.getProperty("ip") ,是界面上 ，属性 里面设置的ip 和 port
    String commName=vehicle.getProperty("commName");
    String ip=vehicle.getProperty("ip");
    Integer port=Integer.parseInt(vehicle.getProperty("port"));
    //bind local port
    Integer bindPort=Integer.parseInt(vehicle.getProperty("bindPort"));


    this.comm = kccommunicationFactory.createUDPService(
        commName,
        ip,
        port,
        bindPort
    );
    Communications.setInstance(commName,comm);

    System.out.println("initial  vehicleName"+vehicle.getName()+",comm hashCode:"+this.comm.hashCode()+",commName: " + commName+", ip: " + ip+", port: " + port+", bindPort: " + bindPort);

    HashMap<String, Communication> kcCommunicationHashMap = Communications.getKcCommunicationHashMap();
    for (Communication comm : kcCommunicationHashMap.values()) {
      System.out.println("kcCommunicationHashMap Communication: " + comm.hashCode()+", getProtocolName: " + comm.getProtocolName());
    }
    //结束
  }

  @Override
  public boolean isInitialized() {
    return initialized;
  }

  @Override
  public void terminate() {
    if (!isInitialized()) {
      return;
    }
    super.terminate();
    initialized = false;
  }



  /**
   * 当前选中的代码实现了 BasicVehicleCommAdapter 类对 PropertyChangeListener 接口中 propertyChange 方法的重写，
   * 其作用是监听 VehicleProcessModel 属性变化事件，并在特定事件发生时触发命令调度任务。以下是详细解释：
   * */
  @Override
  public void propertyChange(PropertyChangeEvent evt) {
    super.propertyChange(evt);

    if (!((evt.getSource()) instanceof KCLoopbackVehicleModel)) {
      return;
    }

    //监听到 车辆负载出现变化 event
    if (Objects.equals(
        evt.getPropertyName(),
        VehicleProcessModel.Attribute.LOAD_HANDLING_DEVICES.name()
    )) {
      if (!getProcessModel().getLoadHandlingDevices().isEmpty()
          && getProcessModel().getLoadHandlingDevices().get(0).isFull()) {
        loadState = LoadState.FULL;
        getProcessModel().setBoundingBox(
            getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthLoaded())
        );
      }
      else {
        loadState = LoadState.EMPTY;
        getProcessModel().setBoundingBox(
            getProcessModel().getBoundingBox().withLength(configuration.vehicleLengthUnloaded())
        );
      }
    }

    //监听到 单车模式的修改 event
    if (Objects.equals(
        evt.getPropertyName(),
        KCLoopbackVehicleModel.Attribute.SINGLE_STEP_MODE.name())
    ) {
      // When switching from single step mode to automatic mode and there are commands to be
      // processed, ensure that we start/continue processing them.
      if (!getProcessModel().isSingleStepModeEnabled()
          && !getSentCommands().isEmpty()
          && !isSimulationRunning) {
        isSimulationRunning = true;
        ((ExecutorService) getExecutor()).submit(
            () -> startVehicle(getSentCommands().peek())
        );
      }
    }


    //监听到 execute command 命令
    if (
        Objects.equals(
          evt.getPropertyName(),
          VehicleProcessModel.Attribute.COMMAND_EXECUTED.name()
        )
    ) {
      //todo 执行移动命令
      System.out.println("执行移动命令");
    }

    //todo  扩展
  }

  @Override
  public synchronized void enable() {
    if (isEnabled()) {
      return;
    }
    System.out.println("kcloopbackCommAdapter enabled");
    kcafVehicleTask = new KCVehicleTask();
    afListenerExecutor.execute(kcafVehicleTask);
    super.enable();
  }

  @Override
  public synchronized void disable() {
    if (!isEnabled()) {
      return;
    }
    afListenerExecutor.shutdownNow();
    super.disable();
  }

  @Override
  public KCLoopbackVehicleModel getProcessModel() {
    return (KCLoopbackVehicleModel) super.getProcessModel();
  }

  @Override
  public synchronized void sendCommand(MovementCommand cmd) {
    requireNonNull(cmd, "cmd");

    // Start the simulation task if we're not in single step mode and not simulating already.
    if (!getProcessModel().isSingleStepModeEnabled()
        && !isSimulationRunning) {
      isSimulationRunning = true;
      // The command is added to the sent queue after this method returns. Therefore
      // we have to explicitly start the simulation like this.
      if (getSentCommands().isEmpty()) {
        ((ExecutorService) getExecutor()).submit(() -> startVehicle(cmd));
      }
      else {
        ((ExecutorService) getExecutor()).submit(
            () -> startVehicle(getSentCommands().peek())
        );
      }
    }
  }

  @Override
  public void onVehiclePaused(boolean paused) {
    getProcessModel().setVehiclePaused(paused);
  }

  @Override
  public void processMessage(Object message) {
  }

  @Override
  public synchronized void initVehiclePosition(String newPos) {
    ((ExecutorService) getExecutor()).submit(() -> getProcessModel().setPosition(newPos));
  }

  @Override
  public synchronized ExplainedBoolean canProcess(TransportOrder order) {
    requireNonNull(order, "order");

    return canProcess(
        order.getFutureDriveOrders().stream()
            .map(driveOrder -> driveOrder.getDestination().getOperation())
            .collect(Collectors.toList())
    );
  }

  private ExplainedBoolean canProcess(List<String> operations) {
    requireNonNull(operations, "operations");

    LOG.debug("{}: Checking processability of {}...", getName(), operations);
    boolean canProcess = true;
    String reason = "";

    // Do NOT require the vehicle to be IDLE or CHARGING here!
    // That would mean a vehicle moving to a parking position or recharging location would always
    // have to finish that order first, which would render a transport order's dispensable flag
    // useless.
    boolean loaded = loadState == LoadState.FULL;
    Iterator<String> opIter = operations.iterator();
    while (canProcess && opIter.hasNext()) {
      final String nextOp = opIter.next();
      // If we're loaded, we cannot load another piece, but could unload.
      if (loaded) {
        if (nextOp.startsWith(getProcessModel().getLoadOperation())) {
          canProcess = false;
          reason = LOAD_OPERATION_CONFLICT;
        }
        else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) {
          loaded = false;
        }
      } // If we're not loaded, we could load, but not unload.
      else if (nextOp.startsWith(getProcessModel().getLoadOperation())) {
        loaded = true;
      }
      else if (nextOp.startsWith(getProcessModel().getUnloadOperation())) {
        canProcess = false;
        reason = UNLOAD_OPERATION_CONFLICT;
      }
    }
    if (!canProcess) {
      LOG.debug("{}: Cannot process {}, reason: '{}'", getName(), operations, reason);
    }
    return new ExplainedBoolean(canProcess, reason);
  }

  @Override
  protected synchronized void connectVehicle() {
  }

  @Override
  protected synchronized void disconnectVehicle() {
  }

  @Override
  protected synchronized boolean isVehicleConnected() {
    return true;
  }

  @Override
  protected VehicleProcessModelTO createCustomTransferableProcessModel() {
    return new KCLoopbackVehicleModelTO()
        .setLoadOperation(getProcessModel().getLoadOperation())
        .setMaxAcceleration(getProcessModel().getMaxAcceleration())
        .setMaxDeceleration(getProcessModel().getMaxDecceleration())
        .setMaxFwdVelocity(getProcessModel().getMaxFwdVelocity())
        .setMaxRevVelocity(getProcessModel().getMaxRevVelocity())
        .setOperatingTime(getProcessModel().getOperatingTime())
        .setSingleStepModeEnabled(getProcessModel().isSingleStepModeEnabled())
        .setUnloadOperation(getProcessModel().getUnloadOperation())
        .setVehiclePaused(getProcessModel().isVehiclePaused());
  }

  /**
   * 这个是界面上，通过按钮触发的。
   * Triggers a step in single step mode.
   * 是这里被调用的， 然后startVehicleSimulation() 方法开始循环
   */
  public synchronized void trigger() {
    if (
        getProcessModel().isSingleStepModeEnabled()
        && !getSentCommands().isEmpty()
        && !isSimulationRunning)
    {
      isSimulationRunning = true;
      ((ExecutorService) getExecutor()).submit(
          () -> startVehicle(getSentCommands().peek())
      );
    }
  }

  private void startVehicle(MovementCommand command) {
    LOG.debug("Starting vehicle simulation for command: {}", command);
    Step step = command.getStep();
    getProcessModel().setState(Vehicle.State.EXECUTING);

    if (step.getPath() == null) {
      LOG.debug("Starting operation simulation...");
      getExecutor().schedule(
          () -> operation(command, 0),
          SIMULATION_PERIOD,
          TimeUnit.MILLISECONDS
      );
    }
    else {
      getProcessModel().getVelocityController().addWayEntry(
          new WayEntry(
              step.getPath().getLength(),
              maxVelocity(step),
              step.getDestinationPoint().getName(),
              step.getVehicleOrientation()
          )
      );

      LOG.debug("Starting movement simulation...");
      getExecutor().schedule(
          () -> movement(command),
          SIMULATION_PERIOD,
          TimeUnit.MILLISECONDS
      );
    }
  }

  private int maxVelocity(Step step) {
    return (step.getVehicleOrientation() == Vehicle.Orientation.BACKWARD)
        ? step.getPath().getMaxReverseVelocity()
        : step.getPath().getMaxVelocity();
  }



  /**
   * Simulate the operation part of a movement command.
   *
   * @param command The command to simulate.
   * @param timePassed The amount of time passed since starting the simulation.
   */
  private void operation(
      MovementCommand command,
      int timePassed
  ) {
    if (timePassed < getProcessModel().getOperatingTime()) {
      getProcessModel().getVelocityController().advanceTime(getSimulationTimeStep());
      getExecutor().schedule(
          () -> operation(command, timePassed + getSimulationTimeStep()),
          SIMULATION_PERIOD,
          TimeUnit.MILLISECONDS
      );
    }
    else {
      LOG.debug("Operation simulation finished.");
      finishMovementCommand(command);
      String operation = command.getOperation();
      if (operation.equals(getProcessModel().getLoadOperation())) {
        // Update load handling devices as defined by this operation
        getProcessModel().setLoadHandlingDevices(
            Arrays.asList(new LoadHandlingDevice(LHD_NAME, true))
        );
        simulateNextCommand();
      }
      else if (operation.equals(getProcessModel().getUnloadOperation())) {
        getProcessModel().setLoadHandlingDevices(
            Arrays.asList(new LoadHandlingDevice(LHD_NAME, false))
        );
        simulateNextCommand();
      }
      else if (operation.equals(this.getRechargeOperation())) {
        LOG.debug("Starting recharge simulation...");
        finishMovementCommand(command);
        getProcessModel().setState(Vehicle.State.CHARGING);
        getExecutor().schedule(
            () -> charging(
                getProcessModel().getPosition(),
                getProcessModel().getEnergyLevel()
            ),
            SIMULATION_PERIOD,
            TimeUnit.MILLISECONDS
        );
      }
      else {
        simulateNextCommand();
      }
    }
  }

  /**
   * Simulate recharging the vehicle.
   *
   * @param rechargePosition The vehicle position where the recharge simulation was started.
   * @param rechargePercentage The recharge percentage of the vehicle while it is charging.
   */
  private void charging(
      String rechargePosition,
      float rechargePercentage
  ) {
    if (!getSentCommands().isEmpty()) {
      LOG.debug("Aborting recharge operation, vehicle has an order...");
      simulateNextCommand();
      return;
    }

    if (getProcessModel().getState() != Vehicle.State.CHARGING) {
      LOG.debug("Aborting recharge operation, vehicle no longer charging state...");
      simulateNextCommand();
      return;
    }

    if (!Objects.equals(getProcessModel().getPosition(), rechargePosition)) {
      LOG.debug("Aborting recharge operation, vehicle position changed...");
      simulateNextCommand();
      return;
    }
    if (nextChargePercentage(rechargePercentage) < 100.0) {
      getProcessModel().setEnergyLevel((int) rechargePercentage);
      getExecutor().schedule(
          () -> charging(rechargePosition, nextChargePercentage(rechargePercentage)),
          SIMULATION_PERIOD,
          TimeUnit.MILLISECONDS
      );
    }
    else {
      LOG.debug("Finishing recharge operation, vehicle at 100%...");
      getProcessModel().setEnergyLevel(100);
      simulateNextCommand();
    }
  }

  private float nextChargePercentage(float basePercentage) {
    return basePercentage
        + (float) (configuration.rechargePercentagePerSecond() / 1000.0) * SIMULATION_PERIOD;
  }

  private void finishMovementCommand(MovementCommand command) {
    //Set the vehicle state to idle
    if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
      getProcessModel().setState(Vehicle.State.IDLE);
    }
    if (Objects.equals(getSentCommands().peek(), command)) {
      // Let the comm adapter know we have finished this command.
      getProcessModel().commandExecuted(getSentCommands().poll());
    }
    else {
      LOG.warn(
          "{}: Simulated command not oldest in sent queue: {} != {}",
          getName(),
          command,
          getSentCommands().peek()
      );
    }
  }

  void simulateNextCommand() {
    if (getSentCommands().isEmpty() || getProcessModel().isSingleStepModeEnabled()) {
      LOG.debug("Vehicle simulation is done.");
      getProcessModel().setState(Vehicle.State.IDLE);
      isSimulationRunning = false;
    }
    else {
      LOG.debug("Triggering simulation for next command: {}", getSentCommands().peek());
      ((ExecutorService) getExecutor()).submit(
          () -> startVehicle(getSentCommands().peek())
      );
    }
  }

  private int getSimulationTimeStep() {
    return (int) (SIMULATION_PERIOD * configuration.simulationTimeFactor());
  }



  private QueryRobotStatusRsp get0xAF(){
    //0xAF(查询机器人状态)
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_QUERY_ROBOT_STATUS);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if(rcv.isOk()){
      QueryRobotStatusRsp queryRobotStatusRsp = new QueryRobotStatusRsp(rcv.getDataBytes());
      return queryRobotStatusRsp;
    }else {
      System.out.println();
      System.out.println("received transationId : "+ "isok:"+rcv.isOk());
      return null;
    }
  }

  /**
   * desc:更新 ：
   *          1.电池电量
   *          2.更新 getProcessModel 最新的点位
   * */
  private void updateVehicleModelByOxAF(QueryRobotStatusRsp queryRobotStatusRsp) {
    try {
      //最后上报时间
      System.out.println("0xAF sub success");
      //AGV状态订阅
      System.out.println();

      //设置小车状态
      byte agvStatus = queryRobotStatusRsp.runningStatusInfo.agvStatus;
      if (agvStatus == (byte)0x00) {
        getProcessModel().setState(Vehicle.State.IDLE);
      }else if (agvStatus == (byte)0x01) {
        getProcessModel().setState(Vehicle.State.EXECUTING);
      }

      //电量--目前无电量返回，设置一个随机值
      float batteryPercentage = queryRobotStatusRsp.batteryStatusInfo.batteryPercentage;
      getProcessModel().setEnergyLevel(89);

      //设置AGV最后一个点位置,不设置最后经过点opentcs无法调度
      String vehicleNowPosition = getProcessModel().getPosition();
      String lastPassPointId = (queryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
      if (vehicleNowPosition == null || !vehicleNowPosition.equals(lastPassPointId)) {
        if ("0".equals(lastPassPointId)) {
          //最终经过点为0手动设置当前位置
          getProcessModel().setPosition("0");
        } else {
          getProcessModel().setPosition(lastPassPointId);
        }
      }
    } catch (Exception e) {
      throw new RuntimeException("processMessage_messageExecutorPool:" + e);
    }
  }

  /**
   * Simulate the movement part of a MovementCommand.
   *
   * @param command The command to simulate.
   */
  private void movement(MovementCommand command) {
    if (!getProcessModel().getVelocityController().hasWayEntries()) {
      return;
    }

    //上一个节点
    WayEntry prevWayEntry = getProcessModel().getVelocityController().getCurrentWayEntry();
    getProcessModel().getVelocityController().advanceTime(getSimulationTimeStep());

    WayEntry currentWayEntry = getProcessModel().getVelocityController().getCurrentWayEntry();
    //if we are still on the same way entry then reschedule to do it again
    if (prevWayEntry == currentWayEntry) {
      getExecutor().schedule(
          () -> movement(command),
          SIMULATION_PERIOD,
          TimeUnit.MILLISECONDS
      );
    }
    else {
      //if the way enties are different then we have finished this step
      //and we can move on.
      getProcessModel().setPosition(prevWayEntry.getDestPointName());
      LOG.debug("Movement simulation finished.");
      if (!command.hasEmptyOperation()) {
        LOG.debug("Starting operation simulation...");
        getExecutor().schedule(
            () -> operation(command, 0),
            SIMULATION_PERIOD,
            TimeUnit.MILLISECONDS
        );
      }
      else {
        finishMovementCommand(command);
        simulateNextCommand();
      }
    }
  }

  private class KCVehicleTask extends CyclicTask {
    private int simAdvanceTime;

    private KCVehicleTask() {
      super(500);
    }

    @Override
    protected void runActualTask() {
      try {
        //todo 1.获取当前位置，并且set当前位置；2.获取当前状态，并且set当前状态
        QueryRobotStatusRsp queryRobotStatusRsp = get0xAF();
        updateVehicleModelByOxAF(queryRobotStatusRsp);

        final MovementCommand curCommand;
        synchronized (KCLoopbackCommunicationAdapter.this) {
          curCommand = getSentCommands().peek();
        }
        simAdvanceTime = (int) (ADVANCE_TIME * 1.0);
        if (curCommand == null) {
          Uninterruptibles.sleepUninterruptibly(ADVANCE_TIME, TimeUnit.MILLISECONDS);
          getProcessModel().getVelocityController().advanceTime(simAdvanceTime);
        } else {
          // If we were told to move somewhere, simulate the journey.
          LOG.debug("Processing MovementCommand...");
          // Simulate the movement.
          kcMovement(curCommand);
          // Simulate processing of an operation.
          // 注意：是在哪里 setCommand 里的 Operation 指令的。
          if (!curCommand.hasEmptyOperation()) {
            kcOperation(curCommand);
          }
          LOG.debug("Processed MovementCommand.");

          if (!isTerminated()) {
            // Set the vehicle's state back to IDLE, but only if there aren't
            // any more movements to be processed.
            if (getSentCommands().size() <= 1 && getUnsentCommands().isEmpty()) {
              getProcessModel().setState(Vehicle.State.IDLE);
            }
            // Update GUI.
            synchronized (KCLoopbackCommunicationAdapter.this) {
              MovementCommand sentCmd = getSentCommands().poll();
              // If the command queue was cleared in the meantime, the kernel
              // might be surprised to hear we executed a command we shouldn't
              // have, so we only peek() at the beginning of this method and
              // poll() here. If sentCmd is null, the queue was probably cleared
              // and we shouldn't report anything back.
              if (sentCmd != null && sentCmd.equals(curCommand)) {
                // Let the vehicle manager know we've finished this command.
                getProcessModel().commandExecuted(curCommand);
                KCLoopbackCommunicationAdapter.this.notify();
              }
            }
          }
        }
      } catch (Exception ex) {
        LOG.error(ex.getMessage());
      }
    }

    private String getSourcePointName(Step step) {
      String sourcePointName = null;
      if (step.getSourcePoint() == null) {
        //起点为空，不下发路径
        System.out.println("sourcePointName is null");
        return sourcePointName;
      }
      return step.getSourcePoint().getName();
    }

    /**
     * 发送移动指令到KC
     * @param command 指令内容
     */
    private void sendMoveCmdToKc(MovementCommand command) {
      //订单ID
      String orderName = command.getTransportOrder().getName();

      //下发起点
      String sourcePointName = getSourcePointName(command.getStep());
      if (sourcePointName == null) {
        return;
      }

      //下发终点
      String destinationPointName = command.getStep().getDestinationPoint().getName();

      //当前点位操作
      String operation = command.getOperation();

      //AGV控制器执行命令。为实现不停车导航，所以指令下发不进行阻塞
      HybridNavigation.command(orderName, sourcePointName, destinationPointName, operation);
    }

    private void kcMovement(MovementCommand movement) throws Exception {
      Step step = movement.getStep();
      if (step.getPath() == null) {
        return;
      }
      //方向就是 是前进还是后退
      Vehicle.Orientation orientation = step.getVehicleOrientation();
      long pathLength = step.getPath().getLength();
      int maxVelocity;
      switch (orientation) {
        case BACKWARD:
          maxVelocity = step.getPath().getMaxReverseVelocity();
          break;
        default:
          maxVelocity = step.getPath().getMaxVelocity();
          break;
      }
      String targetPointName = step.getDestinationPoint().getName();

      getProcessModel().setState(Vehicle.State.EXECUTING);
      String currentPoint = step.getSourcePoint().getName();;
      int currentStatus = 0;

      //agv.sendPath(Integer.parseInt(targetPointName));
      //注意：这里movement 是哪里设置起点
      sendMoveCmdToKc(movement);

      //0xAE 指令
      while (!currentPoint.equals(targetPointName) && !isTerminated()) {
        QueryRobotStatusRsp queryRobotStatusRsp = get0xAF();

        if (queryRobotStatusRsp == null) {
          Thread.sleep(200);
          continue;
        }
        currentPoint = (queryRobotStatusRsp.locationStatusInfo.lastPassPointId).toString();
        getProcessModel().setPosition(currentPoint);


        byte agvStatus = queryRobotStatusRsp.runningStatusInfo.agvStatus;
        if (agvStatus == (byte)0x00) {
          getProcessModel().setState(Vehicle.State.IDLE);
        }else if (agvStatus == (byte)0x01) {
          getProcessModel().setState(Vehicle.State.EXECUTING);
        }
      }
    }



    private void watingStatusForTarget(byte targetStatus){
      while (true) {
        //AGV运行中不能执行动作进入阻塞状态！！！
        QueryRobotStatusRsp qryRobotStatusRsp = get0xAF();
        if (qryRobotStatusRsp.runningStatusInfo.agvStatus == targetStatus ) {
          System.out.println("-operationExec AGV exec operation");
          break;
        }

        try {
          //AGV运行中，阻塞500ms后轮询
          System.out.println("-operationExec AGV IS Running, Thread sleep 500ms");
          Thread.sleep(500);
        } catch (InterruptedException e) {
          // 处理中断异常，重置中断状态
          Thread.currentThread().interrupt();
        }
      }
    }

    /**
     * Simulates an operation.
     * @throws InterruptedException If an exception occured while simulating
     */
    private void kcOperation(MovementCommand command) {

      watingStatusForTarget((byte)0x00);

      //获取动作
      String operation = command.getOperation();

      //下发立即动作指令
      if ("LIFT".equals(operation)) {
        System.out.println("-Operation exec LoadOperation.");
        LOG.debug("-Operation exec LoadOperation.");
        float height = 1.1f;
        byte modeOfMovement = 0x01;
        ActImmediately.allotsOperation(height, modeOfMovement);
      } else if ("DECLINE".equals(operation)) {
        System.out.println("-Operation exec UnloadOperation.");
        LOG.debug("-Operation exec UnloadOperation.");
        float height = 0.1f;
        byte modeOfMovement = 0x02;
        //降叉齿指令
        ActImmediately.allotsOperation(height, modeOfMovement);
      } else if (operation.equals(getRechargeOperation())) {
        System.out.println("-Operation exec RechargeOperation.");
        LOG.debug("-Operation exec RechargeOperation.");
        //执行充电指令
      } else {
        System.out.println("-NOT EXECUTED OPERATION:" + operation);
        LOG.debug("-NOT EXECUTED OPERATION:" + operation);
      }
    }
  }


  /**
   * The vehicle's possible load states.
   */
  private enum LoadState {
    EMPTY,
    FULL;
  }
}
